#include "mpu6050.h"

// MPU6050 寄存器地址
#define MPU6050_ADDR 0x68
#define ACCEL_XOUT_H 0x3B
#define PWR_MGMT_1 0x6B

// 跌倒和静止检测参数
#define FALL_ACCEL_THRESHOLD 2.5f // g
#define STILL_ACCEL_DELTA 0.05f   // g
#define STILL_TIME_MS 5000        // 5秒

MPU6050::MPU6050(TwoWire* wire)
    : mpuWire(wire), fallDetected(false), stillDetected(false), lastMoveTime(0), lastAccelMag(0.0f), fallEvent(false), stillEvent(false) {}

bool MPU6050::begin() {
    mpuWire->beginTransmission(MPU6050_ADDR);
    mpuWire->write(PWR_MGMT_1);
    mpuWire->write(0);
    return mpuWire->endTransmission() == 0;
}

void MPU6050::update() {
    mpuWire->beginTransmission(MPU6050_ADDR);
    mpuWire->write(ACCEL_XOUT_H);
    mpuWire->endTransmission(false);
    mpuWire->requestFrom(MPU6050_ADDR, 6, true);
    int16_t ax = (mpuWire->read() << 8) | mpuWire->read();
    int16_t ay = (mpuWire->read() << 8) | mpuWire->read();
    int16_t az = (mpuWire->read() << 8) | mpuWire->read();
    float accelX = ax / 16384.0f;
    float accelY = ay / 16384.0f;
    float accelZ = az / 16384.0f;
    float accelMag = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);

    // 跌倒检测
    bool lastFall = fallDetected;
    fallDetected = (accelMag > FALL_ACCEL_THRESHOLD);
    if (fallDetected && !lastFall) fallEvent = true;

    // 静止检测
    bool lastStill = stillDetected;
    if (fabs(accelMag - lastAccelMag) > STILL_ACCEL_DELTA) {
        lastMoveTime = millis();
        stillDetected = false;
    } else if (millis() - lastMoveTime > STILL_TIME_MS) {
        stillDetected = true;
    }
    if (stillDetected && !lastStill) stillEvent = true;
    lastAccelMag = accelMag;
}

void MPU6050::clearEvent() {
    fallEvent = false;
    stillEvent = false;
}

int MPU6050::isFallDetected() const {
    return fallDetected;
}

int MPU6050::isStillDetected() const {
    return stillDetected;
}
